package geotransform.tools;

import java.util.*;
import geotransform.*;
import geotransform.coords.*;
import geotransform.datum.*;
import geotransform.ellipsoids.*;
import geotransform.transforms.*;

public class ConvertDatum {
	public Gdc_Coord_3d wgs84, sad69;
	
	private static double la1R,lo1R;
	private static double dX, dY, dZ, a1, f1, a2, f2;
	private static double e1_2, e2_2, dA, dF, n1, m1, dLa, dLo;
	
	public static Gdc_Coord_3d WGS84toSAD69(Gdc_Coord_3d wgs84) throws Exception{
		dX = 66.87;
		dY = -4.37;
		dZ = 38.52;
		a1 = 6378137;
		f1 = 0.0033528107;
		a2 = 6378160;
		f2 = 0.0033528919;
		return convert(wgs84);
	}
	
	public static Gdc_Coord_3d SAD69toWGS84(Gdc_Coord_3d sad69) throws Exception{
		dX = -66.87;
		dY = 4.37;
		dZ = -38.52;
		a1 = 6378160;
		f1 = 0.0033528919;
		a2 = 6378137;
		f2 = 0.0033528107;
		return convert(sad69);
	}
	
	public static Gdc_Coord_3d convert(Gdc_Coord_3d coord) throws Exception{
		la1R = Math.toRadians(coord.latitude);
		lo1R = Math.toRadians(coord.longitude);
		
		e1_2 = f1*(2-f1);
		e2_2 = f2*(2-f2);
	    
	    dA = a2-a1;
	    dF = f2-f1;
	    
	    n1 = a1/(Math.sqrt(1 - e1_2 * (Math.sin(la1R) * Math.sin(la1R))));
	    m1 = (a1*(1-e1_2))/(Math.pow((1 - e1_2*  (Math.sin(la1R) * Math.sin(la1R))),1.5));
	    
	    dLa = ((a1*dF + f1*dA) * Math.sin(2*la1R) - (dX * Math.sin(la1R) * Math.cos(lo1R)) - (dY * Math.sin(la1R) * Math.sin(lo1R)) + (dZ* Math.cos(la1R))) * (1/m1) * (180/Math.PI);
	    dLo = ((-dX) * Math.sin(lo1R) + dY * Math.cos(lo1R)) * (1/(n1*Math.cos(la1R))) * (180/Math.PI);
	    return new Gdc_Coord_3d(coord.latitude+dLa,coord.longitude+dLo,coord.elevation);
	}
}


